import socket
import sys, re, time
import thread

ROBOT_ENABLE = 0
ROBOT_DISABLE = 1
ROBOT_AUTO = 2

def printReply(connection, packet):
    print 'Reply from Team %d' % connection.teamNo
    print '| Mode:', ord(packet[1])
    print '| Master version:', (ord(packet[2]) << 8) | ord(packet[3])
    print '| User version:', (ord(packet[4]) << 8) | ord(packet[5])

class FieldControlConnection:

    # Create a socket that connects to robot at IP, port 3140 (field control)
    def __init__(self, teamNo, callback=None):
        def update(connection, f):
            while self.threadRun:
                try:
                    packet = connection.socket.recv(8)
                    if packet[0] == 'G': f(connection, packet)

                # No packet waiting; sleep 200ms and try again
                except socket.error:
                    time.sleep(0.2)

        self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.ip = '10.0.' + str(teamNo) + '.2'
        self.teamNo = teamNo

        # Make the source port deterministic so that we can reconnect
        # (robot socket binds to first source port it receives from)
        self.socket.bind(('', 60000 + teamNo))
        self.socket.connect((self.ip, 3140))
        self.robotStatus = ROBOT_ENABLE

        # If a callback function was provided, start a thread that calls
        # the callback
        self.socket.setblocking(0)
        self.threadRun = True
        if callback:
            self.updateThread = thread.start_new_thread(update, (self, callback))

    # Send a field control packet
    def sendPacket(self):
        string = 'F' + chr(self.robotStatus)
        self.socket.send(string)

    # Close the socket
    def close(self):
        self.threadRun = False
        self.socket.close()

    def enable(self):
        self.robotStatus = ROBOT_ENABLE
        self.sendPacket()

    def disable(self):
        self.robotStatus = ROBOT_DISABLE
        self.sendPacket()

    def auto(self):
        self.robotStatus = ROBOT_AUTO
        self.sendPacket()


def main():
    if len(sys.argv) == 1:
        print "Usage: %s TEAM [TEAMS...]" % (sys.argv[0],)
        return

    robots = []
    for i in range(1, len(sys.argv)):
        teamNo = int(sys.argv[i])
        conn = FieldControlConnection(teamNo, printReply)
        robots.append(conn)
        print "%d: %s" % (i, conn.ip)

    try:
        while True:
            print '> ',
            line = sys.stdin.readline()
            m = re.match(r'\s*(e|d|a)\s*(\d+)', line, re.IGNORECASE)
            if m != None:
                cmd = m.group(1)
                index = int(m.group(2)) - 1
                if index >= 0 and index < len(robots):
                    if cmd == 'e':
                        robots[index].enable()
                    elif cmd == 'd':
                        robots[index].disable()
                    elif cmd == 'a':
                        robots[index].auto()
            
    except KeyboardInterrupt:
        print "Exit: sending disable signal to robots"
        for robot in robots:
            robot.disable()
            robot.close()


if __name__ == '__main__':
    main()
